// #include "Encoder.h"
#include "AS5600.h"
#include "FOC.h"
#include "FOC_DEF.h"
extern FOC::sMotor Motor_CH1, Motor_CH2;

void FOC::encoderInit(void){
    AS5600Init();
    Motor_CH1.state_encoder = checkEncoderCH1();
    Motor_CH2.state_encoder = checkEncoderCH2();
}
bool FOC::getEncoderPos(void){
    if (Motor_CH1.state_encoder)
    {
        Motor_CH1.last_pos = Motor_CH1.act_pos;
        Motor_CH1.act_pos = getRowFromAS5600_CH1();
    }
    if (Motor_CH2.state_encoder)
    {
        Motor_CH2.last_pos = Motor_CH2.act_pos;
        Motor_CH2.act_pos = getRowFromAS5600_CH2();
    }
    return 0;
}
float filter_rate = 0.3;
void FOC::getMotorSpeed(void){
    if (Motor_CH1.act_pos> 3072 && Motor_CH1.last_pos < 1024)
    {
        Motor_CH1.act_speed = Motor_CH1.act_speed*filter_rate + (Motor_CH1.act_pos-Motor_CH1.last_pos-4096)*(1-filter_rate);
    }
    else if (Motor_CH1.last_pos> 3072 && Motor_CH1.act_pos < 1024)
    {
        Motor_CH1.act_speed = Motor_CH1.act_speed*filter_rate+(Motor_CH1.act_pos-Motor_CH1.last_pos+4096)*(1-filter_rate);
    }
    else Motor_CH1.act_speed = Motor_CH1.act_speed*filter_rate+(Motor_CH1.act_pos-Motor_CH1.last_pos)*(1-filter_rate);
    
    if (Motor_CH2.act_pos> 3072 && Motor_CH2.last_pos < 1024)
    {
        Motor_CH2.act_speed = Motor_CH2.act_pos-Motor_CH2.last_pos-4096;
    }
    else if (Motor_CH2.last_pos> 3072 && Motor_CH2.act_pos < 1024)
    {
        Motor_CH2.act_speed = Motor_CH2.act_pos-Motor_CH2.last_pos+4096;
    }
    else Motor_CH2.act_speed = Motor_CH2.act_pos-Motor_CH2.last_pos;
}